#include <ros/ros.h>
#include <string>
#include <sstream>
#include <std_msgs/String.h>
#include "head_arduino_msgs/Analog.h"
#include "head_arduino_msgs/DigitalWrite.h"
#include "head_arduino_msgs/Digital.h"
#include "ros_arduino_msgs/AnalogFloat.h"
#include "check_sensor_status/check_sensor_status.h"
#include "android_app_communication/androidLogMsg.h"

using namespace std;

class Sensor
{
    public:
       string topic;
       bool isEnable;
       int warnValue;
       string speakStr;
};

Sensor lightSensor;
Sensor smokeSensor;
Sensor noiseSensor;
Sensor dropSensor;
Sensor checkBatSensor;

ros::Publisher tts_pub;
ros::Publisher bat_pub;
ros::Publisher androidLog_pub;
ros::ServiceClient alarmClient;
head_arduino_msgs::DigitalWrite alarmSrv;

static int alarmTime = 0;
static int alarmFlag = POWER_OFF;
static int alarmPin = 0;
static int dropSensorInterval = 0;
static int checkBatInterval = 0;
static float maxVoltage = 0.0;
static float minVoltage = 0.0;

string dropSensorLog = "检测到机器人悬空啦";

void pubStrToSpeak(string input)
{
    std_msgs::String msg;
    msg.data = input;
    tts_pub.publish(msg);
}

void pubLogToAndroid(string log)
{
    android_app_communication::androidLogMsg msg;
    msg.message = log;
    androidLog_pub.publish(msg);
}

void setAlarm(int flag)
{
    alarmSrv.request.pin = alarmPin;
    alarmSrv.request.value = flag;
    if(alarmClient.call(alarmSrv))
    {
        ROS_INFO("Set alarm Flag [%d] successfully!", flag);
    }
}

void lightSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
{
    ROS_DEBUG("Get lightSensor value:[%d]", msg->value);
    if((msg->value < lightSensor.warnValue)&&(lightSensor.isEnable))
    {
        pubLogToAndroid(lightSensor.speakStr);
        pubStrToSpeak(lightSensor.speakStr);
    }
}

void smokeSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
{
    ROS_DEBUG("Get smokeSensor value:[%d]", msg->value);
    if((msg->value > smokeSensor.warnValue)&&(smokeSensor.isEnable))
    {
        pubLogToAndroid(smokeSensor.speakStr);
        noiseSensor.isEnable = false;
        alarmFlag = POWER_ON;
        pubStrToSpeak(smokeSensor.speakStr);
        setAlarm(alarmFlag);
    }
}

void noiseSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
{
    ROS_DEBUG("Get noiseSensor value:[%d]", msg->value);
    if((msg->value > noiseSensor.warnValue)&&(noiseSensor.isEnable))
    {
        pubLogToAndroid(noiseSensor.speakStr);
        pubStrToSpeak(noiseSensor.speakStr);
    }
}

void checkBatCB(const ros_arduino_msgs::AnalogFloat::ConstPtr& msg)
{
    int percent = 0;
    stringstream ss;
    string perStr;
    static int checkTime = checkBatInterval;
    percent = (msg->value - minVoltage)/(maxVoltage - minVoltage)*100;
    ss<<percent;
    ss>>perStr;
    ROS_DEBUG("Get checkBatSensor value:[%f],percent:%d", msg->value, percent);
    std_msgs::String batMsg;
    batMsg.data = perStr;
    bat_pub.publish(batMsg);

    if((percent < checkBatSensor.warnValue)&&(checkBatSensor.isEnable))
    {
        ROS_DEBUG("Now checkBat interval:[%d]", checkTime);
        (checkTime > 0)? (--checkTime):(checkTime = checkBatInterval);
        if(checkTime == checkBatInterval)
        {
            pubLogToAndroid(checkBatSensor.speakStr);
            noiseSensor.isEnable = false;
            pubStrToSpeak(checkBatSensor.speakStr+perStr);
            alarmFlag = POWER_ON;
            setAlarm(alarmFlag);
        }
    }
}

void dropSensorCB(const head_arduino_msgs::Digital::ConstPtr& msg)
{
    ROS_DEBUG("Get drop sensor value:[%d]", msg->value);
    static int checkTime = dropSensorInterval;
    if((msg->value == 1)&&(dropSensor.isEnable))
    {
        ROS_INFO("Now dropSensor interval:[%d]", checkTime);
        pubLogToAndroid(dropSensorLog);
        if(checkTime == dropSensorInterval)
        {
            pubStrToSpeak(dropSensor.speakStr);
        }
        (checkTime > 0)? (--checkTime):(checkTime=dropSensorInterval);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "check_sensor_node");
    ros::NodeHandle nodeHandle;
    string alarmService;
    string ttsTopic;
    string batPercentTopic;
    string androidLogTopic;
    ros::Rate loop_rate(1);

    ros::param::get("~alarmService_name", alarmService);
    ros::param::get("~androidLog_topic", androidLogTopic);

    ros::param::get("~tts_topic", ttsTopic);
    ros::param::get("~alarm_pin", alarmPin);
    ros::param::get("~alarm_time", alarmTime);

    ros::param::get("~lightSensor_topic", lightSensor.topic);
    ros::param::get("~isEnable_lightSensor", lightSensor.isEnable);
    ros::param::get("~warning_lightSensor", lightSensor.warnValue);
    ros::param::get("~voice_lightSensor", lightSensor.speakStr);

    ros::param::get("~smokeSensor_topic", smokeSensor.topic);
    ros::param::get("~isEnable_smokeSensor", smokeSensor.isEnable);
    ros::param::get("~warning_smokeSensor", smokeSensor.warnValue);
    ros::param::get("~voice_smokeSensor", smokeSensor.speakStr);

    ros::param::get("~noiseSensor_topic", noiseSensor.topic);
    ros::param::get("~isEnable_noiseSensor", noiseSensor.isEnable);
    ros::param::get("~warning_noiseSensor", noiseSensor.warnValue);
    ros::param::get("~voice_noiseSensor", noiseSensor.speakStr);

    ros::param::get("~dropSensor_topic", dropSensor.topic);
    ros::param::get("~isEnable_dropSensor", dropSensor.isEnable);
    ros::param::get("~warning_dropSensor", dropSensor.warnValue);
    ros::param::get("~voice_dropSensor", dropSensor.speakStr);
    ros::param::get("~dropSensor_interval", dropSensorInterval);

    ros::param::get("~voltage_topic", checkBatSensor.topic);
    ros::param::get("~isEnable_batCheck", checkBatSensor.isEnable);
    ros::param::get("~warning_battery", checkBatSensor.warnValue);
    ros::param::get("~voice_battery", checkBatSensor.speakStr);
    ros::param::get("~checkBat_interval", checkBatInterval);
    ros::param::get("~battery_pub_topic", batPercentTopic);
    ros::param::get("~max_voltage", maxVoltage);
    ros::param::get("~min_voltage", minVoltage);

    ros::Subscriber subLightSensor = nodeHandle.subscribe(lightSensor.topic, 1, lightSensorCB);
    ros::Subscriber subSmokeSensor = nodeHandle.subscribe(smokeSensor.topic, 1, smokeSensorCB);
    ros::Subscriber subNoiseSensor = nodeHandle.subscribe(noiseSensor.topic, 1, noiseSensorCB);
    ros::Subscriber subDropSensor  = nodeHandle.subscribe(dropSensor.topic,  1, dropSensorCB);
    ros::Subscriber subBatSensor   = nodeHandle.subscribe(checkBatSensor.topic, 1, checkBatCB);
    tts_pub = nodeHandle.advertise<std_msgs::String>(ttsTopic, 1);
    bat_pub = nodeHandle.advertise<std_msgs::String>(batPercentTopic, 1);
    androidLog_pub = nodeHandle.advertise<android_app_communication::androidLogMsg>(androidLogTopic, 5);
    alarmClient = nodeHandle.serviceClient<head_arduino_msgs::DigitalWrite>(alarmService);

    while(ros::ok())
    {
        if(alarmFlag)
        {
            sleep(alarmTime);
            alarmFlag = POWER_OFF;
            setAlarm(alarmFlag);
            noiseSensor.isEnable = true;
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

